#include "bsp_uart.h"
#include "bsp_pthread.h"
#include "gpioconfig.h"
#include "config.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>
#include <errno.h>
#include <signal.h>
#include <stdbool.h>
#include <gpiod.h>
int send_data(Uart_t* ctx, unsigned char* data, int len);
int receive_data(Uart_t* ctx, unsigned char* buf, int max_len, int timeout_ms);

#define SEND_INTERVAL 1000       // 发送间隔（毫秒）
#define MAX_RESPONSE_SIZE 1024   // 最大响应数据大小
#define COMMAND_TIMEOUT 2000     // 命令超时时间（毫秒）

// 全局变量
Uart_t* g_uart = NULL;          // 串口上下文指针
bool g_running = true;          // 运行标志



// 发送激光雷达命令
void send_laser_command(Uart_t* uart, unsigned char* cmd, int len) {
    int ret = send_data(uart, cmd, len);
    if (ret < 0) {
        printf("错误: 激光雷达命令发送失败，预期发送 %d 字节\n", len);
    } else {
        printf("信息: 成功发送激光雷达命令（十六进制）: ");
        for (int i = 0; i < len; i++) {
            printf("%02X ", cmd[i]);
        }
        printf("\n");
    }
}

// 处理接收到的响应
void process_response(unsigned char* buf, int len) {
    if (len <= 0) return;
    
    printf("信息: 接收到响应（十六进制，长度: %d）: ", len);
    for (int i = 0; i < len; i++) {
        printf("%02X ", buf[i]);
    }
    printf("\n");
    
    // 这里可以添加响应解析逻辑
    // 例如检查帧头、校验和等
}

// 信号处理函数 - 用于优雅退出
void signal_handler(int signum) {
    printf("信息: 接收到信号 %d，准备退出...\n", signum);
    g_running = false;
}

int main(int argc, char **argv) {
    // 注册信号处理函数
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);
    
    // 初始化日志
    printf("信息: 激光雷达通信程序启动\n");
    LaserBspInit();
    // 初始化串口
    g_uart = uart_init_context("/dev/ttyS0", 38400, 8, 1, 'N');
    if (!g_uart) {
        printf("错误: 串口上下文初始化失败\n");
        return 1;
    }
    
    // 配置并打开串口
    int flags = O_RDWR | O_NOCTTY | O_NONBLOCK;  // 阻塞模式
    if (open_and_configure_uart(g_uart, flags) < 0) {
        printf("错误: 串口配置失败\n");
        uart_free_context(g_uart);
        return 1;
    }
    
    // 显示串口配置信息
    uart_show_data(g_uart);
    
    // 定义激光雷达命令
    unsigned char LASER_CMD_SINGLE[6]  = {0x55, 0xF2, 0x00, 0x00, 0xF2, 0xAA};
    unsigned char LASER_CMD_1Hz[6]     = {0x55, 0xF3, 0x01, 0x00, 0xF2, 0xAA};
    unsigned char LASER_CMD_5Hz[6]     = {0x55, 0xF3, 0x02, 0x00, 0xF1, 0xAA};
    unsigned char LASER_CMD_STOP[6]    = {0x55, 0xF3, 0x00, 0x00, 0xF3, 0xAA};
    unsigned char LASER_CMD_CHECK[6]   = {0x55, 0xF0, 0x00, 0x00, 0xF0, 0xAA};
    
    // 发送初始化命令 - 设置为5Hz模式
    send_laser_command(g_uart, LASER_CMD_5Hz, sizeof(LASER_CMD_5Hz));
    usleep(500 * 1000);  // 等待设备响应
    
    unsigned char response_buf[MAX_RESPONSE_SIZE] = {0};
    
    printf("信息: 激光雷达通信程序已启动，按Ctrl+C退出\n");
    
    // 主循环
    while (g_running) {
        // 发送检查命令
        send_laser_command(g_uart, LASER_CMD_CHECK, sizeof(LASER_CMD_CHECK));
        
        // 接收响应
        int n = receive_data(g_uart, response_buf, MAX_RESPONSE_SIZE, COMMAND_TIMEOUT);
        if (n > 0) {
            process_response(response_buf, n);
        } else if (n == 0) {
            printf("警告: 接收超时，未收到数据\n");
        } else {
            printf("错误: 接收数据失败，错误码: %d\n", n);
        }
        
        // 清空缓冲区
        memset(response_buf, 0, MAX_RESPONSE_SIZE);
        
        // 等待指定间隔
        usleep(SEND_INTERVAL * 1000);
    }
    
    // 程序退出前的清理工作
    printf("信息: 正在停止激光雷达数据采集...\n");
    send_laser_command(g_uart, LASER_CMD_STOP, sizeof(LASER_CMD_STOP));
    usleep(500 * 1000);  // 等待命令生效
    
    // 释放资源
    if (g_uart) {
        uart_free_context(g_uart);
        g_uart = NULL;
    }
    
    printf("信息: 激光雷达通信程序已退出\n");
    return 0;
}